/**
 *
 * File:   bsp_control_at8236.c
 * Description: AT8236电机控制器
 * 使用方法
 * 1.freeRtos 一个独立线程 while 之前 Motor_Init()  初始化电机
 * 2.Motor_Run(rbus_parser)  根据RBUS数据进行电机运动控制(freeRtos里的while循环里运行每隔20MS读取SBUS数据，进行电机运动控制)
 *
 */
#include "bsp_motor_control.h"

#include "app.h"
#include "bsp_def.h"
#include "bsp_mecanum.h"
#include "bsp_motion_sensor.h"
#include "bsp_pid_controller.h"

// 全局电机对象定义
Motor_TIM_HandleTypeDef motor_left_front, motor_right_front, motor_left_back, motor_right_back;
UniRemote_HandleTypeDef hur1 = {0};  // 创建一个标准遥控器
uint8_t pid = 0;
pid_controller_t vel_pid;

// 电机配置数组，简化初始化
static const Motor_Config_t motor_configs[] = {
    {&motor_left_front, BSP_LF_FORWARD_TIM, BSP_LF_FORWARD_CH, BSP_LF_BACKWARD_TIM, BSP_LF_BACKWARD_CH},
    {&motor_right_front, BSP_RF_FORWARD_TIM, BSP_RF_FORWARD_CH, BSP_RF_BACKWARD_TIM, BSP_RF_BACKWARD_CH},
    {&motor_left_back, BSP_LB_FORWARD_TIM, BSP_LB_FORWARD_CH, BSP_LB_BACKWARD_TIM, BSP_LB_BACKWARD_CH},
    {&motor_right_back, BSP_RB_FORWARD_TIM, BSP_RB_FORWARD_CH, BSP_RB_BACKWARD_TIM, BSP_RB_BACKWARD_CH}};

#define MOTOR_COUNT (sizeof(motor_configs) / sizeof(motor_configs[0]))

// 定时器通道配置数组
static const TIM_Config_t tim_configs[] = {
    {BSP_LF_FORWARD_TIM, BSP_LF_FORWARD_CH},
    {BSP_LF_BACKWARD_TIM, BSP_LF_BACKWARD_CH},
    {BSP_RF_FORWARD_TIM, BSP_RF_FORWARD_CH},
    {BSP_RF_BACKWARD_TIM, BSP_RF_BACKWARD_CH},
    {BSP_LB_FORWARD_TIM, BSP_LB_FORWARD_CH},
    {BSP_LB_BACKWARD_TIM, BSP_LB_BACKWARD_CH},
    {BSP_RB_FORWARD_TIM, BSP_RB_FORWARD_CH},
    {BSP_RB_BACKWARD_TIM, BSP_RB_BACKWARD_CH}};

#define TIM_CONFIG_COUNT (sizeof(tim_configs) / sizeof(tim_configs[0]))

void set_motor_speed(Motor_TIM_HandleTypeDef motor, int16_t speed);
void stop_all_motors(void);
void brake_all_motors(void);
void simple_drive(int16_t joy_x, int16_t joy_y);

/************************************************************************* */
pid_controller_t pos_pid;
static int8_t comp_lf = 0, comp_rf = 0, comp_lb = 0, comp_rb = 0;

int16_t simple_pid(int16_t target, int16_t measurement) {
    if (target == 0) {
        return 0;
    } else {
        if (target > measurement) {
            return 1;
        } else {
            return -1;
        }
    }
}

/************************************************************************* */

// 限制PWM值（静态函数，只在当前文件使用）
static int16_t constrain_PWM(int16_t value) {
    if (value > MAX_PWM)
        return MAX_PWM;
    if (value < MIN_PWM)
        return MIN_PWM;
    return value;
}

// 启动所有PWM定时器
static void start_all_pwm_timers(void) {
    for (int i = 0; i < TIM_CONFIG_COUNT; i++) {
        HAL_TIM_PWM_Start_IT(tim_configs[i].tim, tim_configs[i].channel);
    }
}

// 初始化所有电机结构体
static void init_motor_structures(void) {
    for (int i = 0; i < MOTOR_COUNT; i++) {
        const Motor_Config_t* config = &motor_configs[i];
        Motor_TIM_HandleTypeDef* motor = config->motor;

        motor->forward_tim = config->forward_tim;
        motor->forward_ch = config->forward_ch;
        motor->backward_tim = config->backward_tim;
        motor->backward_ch = config->backward_ch;
    }
}

// 初始化电机
void BSP_MotorControl_Init(void) {
    // 启动所有PWM定时器
    start_all_pwm_timers();

    // 初始化所有电机结构体
    init_motor_structures();

    // 初始停止所有电机
    stop_all_motors();

    printf("Motor Initialization Complete. - %d motors configured.\r\n", MOTOR_COUNT);

    // 初始化PID
    pid_init(&vel_pid, PID_MODE_VELOCITY, 0.01f);  // 100Hz控制频率
    pid_set_parameters(&vel_pid, 0.8f, 0.4f, 0.01f);
}

void BSP_MotorControl_Loop(SBUS_HandleTypeDef* hsbus, LiDARFrameTypeDef* ld06_frame) {
    UniRemote_HandleTypeDef* hur = &hur1;  // hur 为标准遥控句柄，所有遥控转化为标准
    SBUS_To_UniRemote(hur, hsbus);         // 将SBUS数据转换成标准数据
    if (hur->switch_rate == RBUS_SWITCH_ON) {
        pid = 1;
    } else {
        pid = 0;
    }

#if DEBUG_SBUS_REMOTE
    SBUS_PrintChannels(hsbus);  // 打印测试
#endif
#if DEBUG_UNI_REMOTE
    RemoteControlPrintChannels(hur);  // DEBUG
#endif
    // 显示雷达距离
    char* distance = "cm ";
    AX_OLED_DispValue(80, 4, ld06_frame->last_round_min_distance / 10, 4, 0, 0);
    AX_OLED_DispStr(110, 4, (uint8_t*)distance, 0);

#if DEBUG_LIDAR_DETECT
    printf("Motor Detect - lidar:%d cm\n", ld06_frame->last_round_min_distance / 10);
#endif
    if (hur->switch_power != RBUS_SWITCH_MID) {  // POWER三种状态 0 停止，1 麦轮 2:胶轮
        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_RESET);
    } else {
        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_SET);
        // 刹车停止所有电机
        brake_all_motors();
        return;
    }

    if (hur->frame_lost || !hur->frame_connected)  // 有摇杆丢失或者没有连接闪红灯
    {
        HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_3);
        // 惯性停止所有电机
        stop_all_motors();
        BuzzerBeep(100, 1);
        return;
    } else {
        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_SET);
    }
    // printf("distance:%d\n", ld06_frame->round_min_distance);                  //DEBUG
    if (ld06_frame->last_round_min_distance < 250 && hur->switch_mode == 2) {
        // 刹车停止所有电机
        brake_all_motors();
        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_RESET);
        osDelay(500);
        BuzzerBeep(50, 3);
        return;
    }

    if (hur->switch_power == RBUS_SWITCH_UP) {  // 麦轮
        Mecanum_Speed mecanum_speed = {0};
        // 计算麦轮速度
        mecanum_caculus(&mecanum_speed, hur);
#if DEBUG_MOTOR_CONTROL
        printf("mecanum:%d, %d, %d, %d\n", mecanum_speed.lf_speed, mecanum_speed.rf_speed, mecanum_speed.lb_speed, mecanum_speed.rb_speed);
#endif
        // 设置所有电机速度
        set_motor_speed(motor_left_front, mecanum_speed.lf_speed);
        set_motor_speed(motor_right_front, mecanum_speed.rf_speed);
        set_motor_speed(motor_left_back, mecanum_speed.lb_speed);
        set_motor_speed(motor_right_back, mecanum_speed.rb_speed);
#if DEBUG_MOTOR_CONTROL
        printf("Mecanum - LF:%3d RF:%3d LB:%3d RB:%3d\r\n", mecanum_speed.lf_speed, mecanum_speed.rf_speed, mecanum_speed.lb_speed, mecanum_speed.rb_speed);
#endif
        // 普通轮
    } else if (hur->switch_power == RBUS_SWITCH_DOWN) {
        simple_drive(hur->left_x, hur->left_y);
    }
}

// 设置单个电机速度,下发每个麦克纳姆轮的速度值
void set_motor_speed(Motor_TIM_HandleTypeDef motor, int16_t speed) {
    uint16_t pwm_value;
    uint16_t forward_pwm = 0;
    uint16_t backward_pwm = 0;

    if (speed > 0) {
        // 前进
        pwm_value = constrain_PWM(speed);
        forward_pwm = pwm_value;
        backward_pwm = 0;
    } else if (speed < 0) {
        // 后退
        pwm_value = constrain_PWM(-speed);
        forward_pwm = 0;
        backward_pwm = pwm_value;
    } else {
        // 停止
        forward_pwm = 0;
        backward_pwm = 0;
    }
    // printf("test:%ld,%ld,%ld,%ld--------------\n",motor.forward_ch,forward_pwm,motor.backward_ch,backward_pwm);
    __HAL_TIM_SET_COMPARE(motor.forward_tim, motor.forward_ch, forward_pwm);
    __HAL_TIM_SET_COMPARE(motor.backward_tim, motor.backward_ch, backward_pwm);
}

// 简单差速控制（适用于普通四轮小车）
void simple_drive(int16_t joy_x, int16_t joy_y) {
    // 死区处理
    if (abs(joy_x) < 5)
        joy_x = 0;
    if (abs(joy_y) < 5)
        joy_y = 0;

    // 计算左右轮速度
    int16_t left_speed = joy_y + joy_x;
    int16_t right_speed = joy_y - joy_x;

    // PID控制
    int16_t pid_lf = 0;
    int16_t pid_rf = 0;
    int16_t pid_lb = 0;
    int16_t pid_rb = 0;

    if (pid == 1) {
        // 简单PID 每次获得正负2的调整
        printf("DETECT:LF%.1f, RF%.1f, LB%.1f, RB%.1f\n", hws.wheelSpeeds[0].speed / 2.9, hws.wheelSpeeds[1].speed / 2.9, hws.wheelSpeeds[2].speed / 2.9, hws.wheelSpeeds[3].speed / 2.9);
        comp_lf += simple_pid(left_speed, hws.wheelSpeeds[0].speed / 2.9);
        comp_rf += simple_pid(right_speed, hws.wheelSpeeds[1].speed / 2.9);
        comp_lb += simple_pid(left_speed, hws.wheelSpeeds[2].speed / 2.9);
        comp_rb += simple_pid(right_speed, hws.wheelSpeeds[3].speed / 2.9);

        pid_lf = left_speed + comp_lf;
        pid_rf = right_speed + comp_rf;
        pid_lb = left_speed + comp_lb;
        pid_rb = right_speed + comp_rb;
        printf("PID COMP:%d, %d, %d, %d\n", comp_lf, comp_rf, comp_lb, comp_rb);


        printf("PID VALUE:%d, %d, %d, %d\n", pid_lf, pid_rf, pid_lb, pid_rb);
        /*
        pid_lf = pid_calculate_velocity(&vel_pid, (float)left_speed, hws.wheelSpeeds[0].speed / 2.5);
        pid_rf = pid_calculate_velocity(&vel_pid, (float)right_speed, hws.wheelSpeeds[1].speed / 2.5);
        pid_lb = pid_calculate_velocity(&vel_pid, (float)left_speed, hws.wheelSpeeds[2].speed / 2.5);
        pid_rb = pid_calculate_velocity(&vel_pid, (float)right_speed, hws.wheelSpeeds[3].speed / 2.5);
        */

        if ((left_speed > 0 && pid_lf < 0) || (left_speed < 0 && pid_lf > 0) || (left_speed == 0)) {
            pid_lf = 0;
            comp_lf = 0;
        }

        if ((right_speed > 0 && pid_rf < 0) || (right_speed < 0 && pid_rf > 0) || (right_speed == 0)) {
            pid_rf = 0;
            comp_rf = 0;
        }

        if ((left_speed > 0 && pid_lb < 0) || (left_speed < 0 && pid_lb > 0) || (left_speed == 0)) {
            pid_lb = 0;
            comp_lb = 0;
        }

        if ((right_speed > 0 && pid_rb < 0) || (right_speed < 0 && pid_rb > 0) || (right_speed == 0)) {
            pid_rb = 0;
            comp_rb = 0;
        }

#if DEBUG_MOTOR_CONTROL
        // 调试信息
        printf("----------------PID MODE--------------------\n");
        printf("Simple -PID LF:%d; Detect:%f, PID_LF:%d.\r\n", left_speed, hws.wheelSpeeds[0].speed / 2.9, pid_lf);
        printf("Simple -PID RF:%d; Detect:%f, PID_RF:%d.\r\n", right_speed, hws.wheelSpeeds[1].speed / 2.9, pid_rf);
        printf("Simple -PID LB:%d; Detect:%f, PID_LB:%d.\r\n", left_speed, hws.wheelSpeeds[2].speed / 2.9, pid_lb);
        printf("Simple -PID RB:%d; Detect:%f, PID_RB:%d.\r\n", right_speed, hws.wheelSpeeds[3].speed / 2.9, pid_rb);
#endif
        // 设置电机速度（前后轮同侧速度相同）
        set_motor_speed(motor_left_front, pid_lf);
        set_motor_speed(motor_left_back, pid_lb);
        set_motor_speed(motor_right_front, pid_rf);
        set_motor_speed(motor_right_back, pid_rb);
    } else {
// 设置电机速度（前后轮同侧速度相同）
#if DEBUG_MOTOR_CONTROL
        printf("-------------NO PID-----------------------\n");
        printf("Simple - LF:%d; RF:%d; LB:%d; RB:%d.\r\n", left_speed, right_speed, left_speed, right_speed);
#endif
        set_motor_speed(motor_left_front, left_speed);
        set_motor_speed(motor_left_back, left_speed);
        set_motor_speed(motor_right_front, right_speed);
        set_motor_speed(motor_right_back, right_speed);
    }
}

// 停止所有电机(惯性)
void stop_all_motors(void) {
    set_motor_speed(motor_left_front, 0);
    set_motor_speed(motor_right_front, 0);
    set_motor_speed(motor_left_back, 0);
    set_motor_speed(motor_right_back, 0);
    // printf("All motors stopped\r\n");
}

// 刹车函数 - 紧急制动
void brake_all_motors(void) {
    // 同时给所有电机的前进和后退通道最大PWM，产生制动效果
    for (int i = 0; i < TIM_CONFIG_COUNT; i++) {
        __HAL_TIM_SET_COMPARE(tim_configs[i].tim, tim_configs[i].channel, MAX_PWM);
    }

    // 制动后延时一段时间再完全停止
    osDelay(50);
    stop_all_motors();
}